import rospy
import numpy as np
import multiprocessing.shared_memory
import multiprocessing

class SharedMemoryCommunicator:
    def __init__(self, name, size, message_type):
        self.name = name
        self.size = size
        self.message_type = message_type
        try:
            self.shm = multiprocessing.shared_memory.SharedMemory(name=self.name, create=False, size=self.size)
        except FileNotFoundError:
            self.create = True
            self.shm = multiprocessing.shared_memory.SharedMemory(name=self.name, create=self.create, size=self.size)

    def read(self):
        msg = self.message_type()
        a = self.shm.buf[:self.size].tobytes()
        msg.deserialize(a)
        return msg

    def write(self, msg):
        buf = io.BytesIO()
        msg.serialize(buf)
        bufData = bytearray(buf.getvalue())
        self.shm.buf[:len(bufData)] = bufData

    def close(self):
        if self.create:
            self.shm.close()
            self.shm.unlink()


if __name__ == '__main__':
    from std_msgs.msg import Header
    rospy.init_node('shared_memory_communicator', anonymous=True)
    multiprocessing.Lock()
    shm = SharedMemoryCommunicator('test_head', 1024, Header)
    print(shm.read())
    shm.close()